#!/usr/bin/env python3

# Create by haibo 2023.9.11
# ros topic monitor

import time
from time import sleep

import rospy
from rospy import rostime
from rostopic import ROSTopicHz
from agv_msgs.msg import AGVEvent

def publishevent(type, action, ecode, msg, pub):
    event = AGVEvent()
    current_time = rospy.Time.now()
    event.header.stamp = current_time
    event.header.frame_id = rospy.get_name()
    event.type = type;
    event.action = action;
    event.ackReq = False;
    event.ecode = ecode;
    event.eventTime = current_time.to_sec();
    event.description = msg;
    pub.publish(event)

def topicmonitor():
    rclpy.init()
    node = rclpy.create_node("topic_monitor")
    try:
        topics = rospy.get_param('/Monitor/topic_monitor/topics')
        frequency = rospy.get_param('/Monitor/topic_monitor/frequency')
        print(topics, frequency)
    except KeyError:
        print ("params value not set")
        quit()

    # create publish
    event_pub = node.create_publisher(AGVEvent, queue_size=10, '/agvevent')
    # set loop rate
    rate = rospy.Rate(frequency)
    last_xvalues_for_calc = 10

    hz_monitors = {}
    lastupdate = {}
    for entry in topics:
        rthz = ROSTopicHz(-1, filter_expr=None)
        sub = node.create_subscription(rospy.AnyMsg, topics[entry]['name'], rthz.callback_hz)
        hz_monitors[topics[entry]['name']] = rthz
        lastupdate[topics[entry]['name']] = 0

    start_timestamp = time.time()
    lastupdate_timestamp = start_timestamp
    publsh_timeout = 0.99 / frequency
    while not rospy.is_shutdown():  # main loop
        topic_timeout = False
        now = time.time()
        for entry in topics:
            n_ = len(hz_monitors[topics[entry]['name']].times)

            if n_ < 2: # wait for 2 cycles
                if now - start_timestamp <= 3: #It will switch in 3 seconds
                    continue
            if n_ == 0:  # no mis received, division by 0 catching
                rospy.logwarn("no new miS for topic %s", topics[entry]['name'])
                value = "Topic " + topics[entry]['name'] + " sends no data"
                publishevent(AGVEvent.ALARM, AGVEvent.ESTOP, AGVEvent.TOPIC_TIME_OUT, value, event_pub)
                lastupdate_timestamp = now
                topic_timeout = True
                continue

            mean = sum(hz_monitors[topics[entry]['name']].times[-last_xvalues_for_calc:]) / last_xvalues_for_calc

            freq = 1. / mean if mean > 0. else 0

            if topics[entry]['frequency'] - 0.1*freq > freq or lastupdate[topics[entry]['name']] == n_:
                topic_timeout = True
                lastupdate_timestamp = now
                if lastupdate[topics[entry]['name']] != n_:
                  rospy.logwarn("Frequency of %s is to low: Expected: %f Actual: %f", topics[entry]['name'], topics[entry]['frequency'], freq)
                  value = "Topic " + topics[entry]['name'] + " is on " + str(freq) + "Hz, Expected: " + str(topics[entry]['frequency'])
                  publishevent(AGVEvent.WARNING, AGVEvent.NONE, AGVEvent.TOPIC_LOW_FREQ, value, event_pub)
                else:
                  rospy.logwarn('%s data stop updating...', topics[entry]['name'])
                  value = "Topic " + topics[entry]['name'] + " stoped, count:" + str(lastupdate[topics[entry]['name']])
                  publishevent(AGVEvent.ALARM, AGVEvent.ESTOP, AGVEvent.TOPIC_TIME_OUT, value, event_pub)
            lastupdate[topics[entry]['name']] = n_

        if not topic_timeout or now - lastupdate_timestamp > publsh_timeout:
            value = "Topic monitor works normally."
            publishevent(AGVEvent.INFO, AGVEvent.NONE, AGVEvent.NONE_EVENT, value, event_pub)
            lastupdate_timestamp = now
        rate.sleep()

if __name__ == '__main__':
    try:
        topicmonitor()
    except rospy.ROSInterruptException:
        pass
